#include "../include/kuka_interfaces.hpp"

bool KR3_interface::getCartesian(double *pose)
{
    CThread::lock();
    for (auto i = 0; i < 7; i++){
        pose[i]=_vrep_pose_T2W[i];
    }
    CThread::unlock();
    return true;
}

bool KR3_interface::setCartesian(const double *pose)
{
    return true;
}

bool KR3_interface::setJoints(const double *joints)
{
    CThread::lock();
    for (auto i = 0; i < 6; i++){
        _joint_cmd[i]=joints[i];
    // std::cout << "master thread " <<  _joint_cmd[i] <<"\n";
    }
    CThread::unlock();
    return true;
}

void KR3_interface::getObj2World(double *pose){
    CThread::lock();
    for (auto i = 0; i < 7; i++){
        pose[i]=_vrep_pose_O2W[i];
    }
    CThread::unlock();
}

bool KR3_interface::getJoints(double *joints){
    return true;
}

KR3_interface::KR3_interface(){
    _max_dist_tran = 0;
    _max_dist_rot = 0;
    _check_zone = new double[3]();
    _safe_zone = new double[6]();
    _joint_cmd = new double[7]();
    _vrep_pose_T2S = new double[7]();
    _vrep_pose_T2W = new double[7]();
    _vrep_init_pose =   new double[7]();
    _vrep_world2base=   new double[7]();
    _vrep_pose_O2W = new double[7]();
}

KR3_interface::~KR3_interface(){
    delete [] _check_zone;
    delete [] _joint_cmd;
    delete [] _safe_zone;
    delete [] _vrep_pose_T2S;
    delete [] _vrep_pose_T2W;
    delete [] _vrep_init_pose;
    delete [] _vrep_world2base;
    delete [] _vrep_pose_O2W;
}